#!/usr/bin/env python
__builtins__.__openravepy_version__ = '0.7'

from openravepy import *
from numpy import *
import time

RaveInitialize()
RaveLoadPlugin('./distancesensor')

env=Environment()
env.Load('scenes/myscene.env.xml')
#env.SetViewer('qtcoin')

#sensor = env.GetSensors()[0]
#while True:
    #print sensor.GetSensorData().positions, sensor.GetSensorData().ranges, sensor.GetSensorData().intensity
    #print "colliding bodies " + sensor.SendCommand('collidingbodies')
    #time.sleep(1)
    
env.SetViewer('qtcoin')

physics = RaveCreatePhysicsEngine(env,'ode')
env.SetPhysicsEngine(physics)

with env:
    env.GetPhysicsEngine().SetGravity(array((0,0,-9.8)))
    robot = env.GetRobots()[0]
    robot.SetController(RaveCreateController(env,'odevelocity'),range(robot.GetDOF()),0)
    env.StopSimulation()
    env.StartSimulation(timestep=0.001)

starttime = time.time()

us = robot.GetAttachedSensor('ultrasonic')
ir = robot.GetAttachedSensor('infrared')

print us

while True:
    velocities = 8*(random.rand(robot.GetDOF())-0.5)
    print 'velocities: ',velocities
    robot.GetController().SendCommand('setvelocity '+' '.join(str(f) for f in velocities))
    
    print us.GetData().positions, us.GetData().ranges, us.GetData().intensity
    print ir.GetData().positions, ir.GetData().ranges, ir.GetData().intensity
    
    #here i have some problems in the c++ implementation...
    #print "colliding bodies " + ir.GetSensor().SendCommand('collidingbodies')
    time.sleep(5)

if __name__=='__main__':
    main()

